#include "adamnet.h"
#include "coleco.h"
#include <cstdio>
#include <QDebug>
#include <QtConcurrentRun>
#include <QFuture>
#include <cstring>




bool isFormatting = false;
static const byte interleave[8] = { 0, 5, 2, 7, 4, 1, 6, 3 };

void AdamFlushCache_CPM()
{
    for (int i = 0; i < savedLEN; ++i) {
        // CRITICAL: Protect PCB area ($FEC0-$FEFF)
        // Stop writing if we reach PCB area
        if (savedBUF >= 0xFEC0 && savedBUF < 0xFF00) {
            // Skip PCB area - don't overwrite it!
            savedBUF = 0xFF00;
        }

        RAM_Memory[savedBUF] = HoldingBuf[i];
        savedBUF++;
    }
}

void UpdateDSK_CPM(byte N, byte Dev, int V)
{
    int I, LEN, SEC;
    int byte_idx;
    int K;
    byte *Data;
    uint32_t block;
    word BUF;  // ← LOKAAL maken! Net zoals in tape!

    if (N >= MAX_DISKS) {
        return;
    }

    // ========================================================================
    // POLLING / BUSY CHECK
    // ========================================================================
    static byte active_disk = 0xFF;  // Welke disk is nu bezig?

    if (V < 0) {
        static int poll_count = 0;

        // CRITICAL: Maintain C940 during polling too!
        RAM_Memory[0xC940] = 0x04 + N;
        RAM_Memory[0xC9E6] = 0x04 + N;

        // AGGRESSIVE: Force $FD6F to $18 whenever it's between $04-$07
        static byte last_fd6f = 0xFF;
        byte current_fd6f = RAM_Memory[0xFD6F];

        // Log FD6F changes
        if (current_fd6f != last_fd6f) {
            qDebug() << "[FD6F-WATCH] Changed: $" << QString("%1").arg(last_fd6f, 2, 16, QChar('0'))
            << "→ $" << QString("%1").arg(current_fd6f, 2, 16, QChar('0'));
            last_fd6f = current_fd6f;
        }

        // AGGRESSIVELY force to $18 whenever stuck between $04-$07
        if (current_fd6f >= 0x04 && current_fd6f < 0x08) {
            RAM_Memory[0xFD6F] = 0x18;
            if (current_fd6f != 0x18) {
                qDebug() << "[FD6F-FORCE] $" << QString("%1").arg(current_fd6f, 2, 16, QChar('0'))
                << "→ $18 (forcing completion)";
            }
        }

        if (active_disk == N && ++poll_count % 1000 == 0) {
            qDebug() << "[DISK-POLL] N=" << N << "Dev=" << Dev
                     << "io_busy=" << io_busy
                     << "active=" << (active_disk == N ? "YES" : "NO");
        }

        // Alleen deze disk pollen als het de actieve is
        if (active_disk == N && io_busy > 0) {
            io_busy--;
            if (io_busy == 0) {
                active_disk = 0xFF;  // Reset
                if (last_command_read) {
                    last_command_read = 0;
                    AdamFlushCache_CPM();
                }

                // Set standard DCB status
                SetDCB(Dev, DCB_CMD_STAT, RSP_STATUS);

                // CRITICAL: CP/M checks DCB status at MULTIPLE locations!
                // Force ALL possible status bytes to $80 (ready/success)
                // DCB base for Dev 2 is around $FExx-$FFxx
                // Aggressively set status bytes
                word dcb_base = 0xFEC0 + (Dev * 21);  // Estimate DCB location
                for (int offset = 0; offset < 21; offset++) {
                    word addr = dcb_base + offset;
                    // If byte looks like a status (0x00-0x0F), force to 0x80
                    if (RAM_Memory[addr] < 0x10) {
                        RAM_Memory[addr] = 0x80;
                    }
                }

                // Also force high memory DCB area
                for (word addr = 0xFF20; addr < 0xFF40; addr++) {
                    if (RAM_Memory[addr] > 0 && RAM_Memory[addr] < 0x10) {
                        RAM_Memory[addr] = 0x80;
                    }
                }

                qDebug() << "[DISK-POLL] IO DONE - N=" << N << "Dev=" << Dev
                         << "- Forced status bytes to $80";
            }
            else {
                SetDCB(Dev, DCB_CMD_STAT, 0x00);
            }
        }
        return;
    }

    // if (V < 0) {
    //     if (io_busy > 0) {
    //         io_busy--;
    //         if (io_busy == 0) {
    //             if (last_command_read) {
    //                 last_command_read = 0;
    //                 AdamFlushCache_CPM();
    //             }

    //             // 1. Update de standaard DCB (voor de vorm)
    //             SetDCB(Dev, DCB_CMD_STAT, RSP_STATUS);

    //             // 2. --- DE AUTOMATISCHE DOORBRAAK ---
    //             // We kijken direct op de drie adressen die je hebt gevonden.
    //             // Als de BIOS daar op 'Busy' (01) staat, zetten we hem op 'Ready' (80).
    //             /*
    //             if (RAM_Memory[0xFED9] == 0x01) {
    //                 RAM_Memory[0xFED9] = 0x80;
    //                 qDebug() << "[AUTO-SYNC] BIOS loop op $FED9 doorbroken!";
    //             }

    //             if (RAM_Memory[0xFEEE] == 0x01) {
    //                 RAM_Memory[0xFEEE] = 0x80;
    //                 qDebug() << "[AUTO-SYNC] BIOS loop op $FEEE doorbroken!";
    //             }

    //             if (RAM_Memory[0xFF03] == 0x01) {
    //                 RAM_Memory[0xFF03] = 0x80;
    //                 qDebug() << "[AUTO-SYNC] BIOS loop op $FF03 doorbroken!";
    //             }
    //             */
    //         }
    //         else {
    //                     SetDCB(Dev, DCB_CMD_STAT, RSP_STATUS);
    //         //            RAM_Memory[0xFED9] = 0xC6; // bypass hier wacht hij eerst
    //              }
    //     }
    //     return;
    // }

    // ========================================================================
    // COMMANDO VERWERKING
    // ========================================================================
    switch (V) {
    case CMD_STATUS:
        io_busy = 0;  // ← Ook hier
        ReportDevice(Dev, 0x0400, 1);
        // 2. Zet de status op 'Succes / Ready' (0x80)
        SetDCB(Dev, DCB_CMD_STAT, RSP_STATUS);
        break;

    case CMD_SOFT_RESET:
        io_busy = 0;  // ← Ook hier
        SetDCB(Dev, DCB_CMD_STAT, RSP_STATUS);
        break;

    case CMD_READ:
    case CMD_WRITE:
        // MINIMAL: Only initialize device variables, NO patches
        if (V == CMD_READ && GetDCBSector(Dev) == 0) {
            byte device_id = 0x04 + N;

            qDebug() << "[CPM-INIT] Sector 0 - init device vars (NO patches)";

            // ONLY initialize variables
            RAM_Memory[0xC940] = device_id;
            RAM_Memory[0xC941] = 0x00;
            RAM_Memory[0xC942] = 0x00;
            RAM_Memory[0xC943] = 0x00;
            RAM_Memory[0xC9E6] = device_id;

            qDebug() << "[CPM-INIT] Set $C940=$C9E6=$"
                     << QString("%1").arg(device_id, 2, 16, QChar('0'));
        }

        qDebug() << "[DSK-CMD]" << (V == CMD_READ ? "READ" : "WRITE")
                 << "N=" << N << "Dev=" << Dev
                 << "Sector=" << GetDCBSector(Dev)
                 << "BUF=" << QString("0x%1").arg(GetDCBBase(Dev), 4, 16, QChar('0'))
                 << "Setting active_disk=" << N << "io_busy=" << (DELAY_IO * 100);

        g_diskSoundActive.store(true, std::memory_order_relaxed);
        io_show_status = (V == CMD_READ) ? 1 : 2;

        SetDCB(Dev, DCB_CMD_STAT, 0x00);
        active_disk = N;  // Markeer deze disk als actief
        io_busy = DELAY_IO * 100;  // ← Per-disk busy counter!

        if (!Disks[N].Data) {
            qDebug() << "[DSK] No disk data! N=" << N;
            break;
        }

        BUF = GetDCBBase(Dev);
        LEN = GetDCBLen(Dev);
        LEN = (LEN < 0x0400) ? LEN : 0x0400;
        SEC = GetDCBSector(Dev);

        // CRITICAL: Only set savedBUF on FIRST read (sector 0)
        // Otherwise keep the accumulated value from previous flushes!
        if (savedBUF == 0 || SEC == 0) {
            savedBUF = BUF;
        }
        savedLEN = LEN;
        block = SEC * 2;

        for (I = 0; I < LEN; I += 512)
        {

            uint32_t physical_sector = (block & (~7)) | interleave[block & 7];

            Data = LinearFDI(&Disks[N], physical_sector);

            if (!Data) break;
            K = (I + 512 > LEN) ? LEN - I : 512;

            if (V == CMD_READ)
            {
                last_command_read = true;
                for (byte_idx = 0; byte_idx < K; ++byte_idx, ++BUF) {
                    HoldingBuf[I + byte_idx] = Data[byte_idx];
                }
            }
            else {
                last_command_read = false;
                for (byte_idx = 0; byte_idx < K; ++byte_idx, ++BUF)
                    Data[byte_idx] = RAM_Memory[BUF & 0xFFFF];
            }
            block++;
        }

        break;
    }
}

void UpdateTAP_CPM(byte N,byte Dev,int V)
{
    int I,J,K,LEN,SEC;
    word BUF;
    byte *Data;

    /* If reading DCB status, stop here */
    static byte active_tape = 0xFF;  // Welke tape is nu bezig?

    if (V < 0)
    {
        // Alleen deze tape pollen als het de actieve is
        if (active_tape == N && io_busy > 0)
        {
            io_busy--;
            if (io_busy == 0)
            {
                active_tape = 0xFF;  // Reset
                // Net klaar: flush (indien READ) en direct READY zetten
                if (last_command_read) {
                    last_command_read = 0;
                    AdamFlushCache();
                }
                SetDCB(Dev, DCB_CMD_STAT, RSP_STATUS);
            }
            else
            {
                // Nog bezig
                SetDCB(Dev, DCB_CMD_STAT, 0x00);
            }
        }
        // GEEN else - laat status zoals die is
        return;
    }

    /* Reset errors, report missing tapes */
    SetDCB(Dev,DCB_NODE_TYPE,(Tapes[N&2].Data? 0x00:0x03)|(Tapes[(N&2)+1].Data? 0x00:0x30));

    /* Depending on the command... */
    switch(V)
    {
    case CMD_STATUS:
        /* Block-based device, 1kB buffer */
        ReportDevice(Dev,0x0400,1);
        break;

    case CMD_SOFT_RESET:
        SetDCB(Dev,DCB_CMD_STAT,RSP_STATUS);
        break;

    case CMD_WRITE:
    case CMD_READ:
        g_tapeSoundActive.store(true, std::memory_order_relaxed);
        io_show_status = (V==CMD_READ) ? 1:2;
        // TODO if (io_show_status == 2) adam_unsaved_data = 1;
        /* Busy status by default */
        SetDCB(Dev,DCB_CMD_STAT,0x00);
        active_tape = N;  // Markeer deze tape als actief
        io_busy = DELAY_IO;
        /* If no tape, stop here */
        if(!Tapes[N].Data) break;
        /* Determine buffer address, length, block number */
        BUF = GetDCBBase(Dev);
        LEN = GetDCBLen(Dev);
        LEN = LEN<0x0400? LEN:0x0400;
        SEC = GetDCBSector(Dev);
        savedBUF = BUF;
        savedLEN = LEN;

        /* For each 512-byte sector... */
        for(I=0, SEC<<=1 ; I<LEN ; ++SEC, I+=0x200)
        {
            /* Get pointer to sector data on tape */
            Data = LinearFDI(&Tapes[N],SEC);
            /* If wrong sector number, stop here */
            if(!Data)
            {
                SetDCB(Dev,DCB_NODE_TYPE,GetDCB(Dev,DCB_NODE_TYPE)|0x02);
                SetDCB(Dev,DCB_SEC_LO, 0);
                SetDCB(Dev,DCB_SEC_HI, 0);
                break;
            }
            /* Read or write sectors */
            K = I+0x200>LEN? LEN-I:0x200;
            if(V==CMD_READ)
            {
                last_command_read = true;
                for(J=0;J<K;++J,++BUF)
                {
                    HoldingBuf[I+J] = Data[J];
                }
            }
            else
            {
                last_command_read = false;
                for(J=0;J<K;++J,++BUF) Data[J] = RAM(BUF);
            }
            /* If disk access failed, stop here */
            if(J<K)
            {
                SetDCB(Dev,DCB_NODE_TYPE,GetDCB(Dev,DCB_NODE_TYPE)|0x06);
                SetDCB(Dev,DCB_SEC_LO, 0);
                SetDCB(Dev,DCB_SEC_HI, 0);
                break;
            }
        }
        /* Done */
        break;
    }
}

void UpdateDCB_CPM(byte Dev, int V)
{

    byte DevID;

    if(!V) return;

    DevID = (GetDCB(Dev,DCB_DEV_NUM)<<4) + (GetDCB(Dev,DCB_ADD_CODE)&0x0F);


    switch(DevID)
    {
    case 0x01: UpdateKBD(Dev,V);break;
    case 0x02: UpdatePRN(Dev,V);break;
    case 0x04:
    case 0x05:
    case 0x06:
    case 0x07: UpdateDSK_CPM(DiskID=DevID-4,Dev,V);break;
    case 0x08:
    case 0x09:
    case 0x18:
    case 0x19: UpdateTAP_CPM((DevID>>4)+((DevID&1)<<1),Dev,V);break;
    case 0x52: UpdateDSK_CPM(DiskID,Dev,-2);break;
    default:
        SetDCB(Dev,DCB_CMD_STAT,RSP_ACK+0x0B);
        break;
    }
}
